/*
 * app_task.c
 *
 *  Created on: Jan 31, 2024
 *      Author: Administrator
 */

#include "app_task.h"
#include "main.h"
#include "app_store.h"
#include "dlom2_sensor.h"
#include "user_mb_app.h"
#include "pid_pmeasure.h"
#include "tim.h"
#include "data_convert.h"
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <stdint.h>
#include <stdbool.h>

pid_pmeasure_ctrl_t *pid_ctrl_ptr;

static float input = 0;
static float output = 0;
static uint16_t pwm_output = 0;
static uint16_t pwm_dir = 0;

void set_pwm_output(uint16_t output) {
	__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, output);
}

void set_pwm_dir_output(uint8_t dir)
{
	if(dir == 0)
	{
		HAL_GPIO_WritePin(Dir_Forward_PIN_GPIO_Port,Dir_Forward_PIN_Pin,SET);
		HAL_GPIO_WritePin(Dir_Reverse_PIN_GPIO_Port,Dir_Reverse_PIN_Pin,RESET);
	}else

	{
		HAL_GPIO_WritePin(Dir_Forward_PIN_GPIO_Port,Dir_Forward_PIN_Pin,RESET);
		HAL_GPIO_WritePin(Dir_Reverse_PIN_GPIO_Port,Dir_Reverse_PIN_Pin,SET);
	}
}
int get_pid_ctrl_state()
{
	return Slave_station.usSRegHoldBuf[Pid_Ctrl_Status_Reg];
}
void app_task_pid_task_init() {

	pid_ctrl_ptr = pid_pmeasure_ctrl_new(&input, &output, &pid_cache.sv,
			pid_cache.kp, pid_cache.ki, pid_cache.kd, P_ON_M, pwm_dir);

	pid_pmeasure_ctrl_set_model(pid_ctrl_ptr, AUTOMATIC);

	pid_pmeasure_ctrl_set_output_limits(pid_ctrl_ptr, pid_cache.out_min,
			pid_cache.out_max);

	pid_pmeasure_ctrl_set_controller_direction(pid_ctrl_ptr, DIRECT);

	HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);

}

void app_task_excute_output() {

	if(get_pid_ctrl_state() != 1)
	{
		return;
	}
	//更新pid参数
	pid_pmeasure_ctrl_set_output_limits(pid_ctrl_ptr, pid_cache.out_min,
			pid_cache.out_max);
	pid_pmeasure_ctrl_set_tunings(pid_ctrl_ptr, pid_cache.kp, pid_cache.ki,
			pid_cache.kd, P_ON_M);

	input = dlom2_sensor_value.temperatureValue;
	if (input >= pid_cache.sv) {
		pid_pmeasure_ctrl_set_controller_direction(pid_ctrl_ptr, DIRECT);
	} else

	{
		pid_pmeasure_ctrl_set_controller_direction(pid_ctrl_ptr, REVERSE);

	}
	//执行计算
	bool ret = pid_pmeasure_ctrl_compute(pid_ctrl_ptr);
	pwm_output = (uint16_t) output;
	set_pwm_dir_output(pwm_dir);
	set_pwm_output(pwm_output);
	printf("pwm output:%d \r\n", pwm_output);
}

void app_task_check_update_init() {

	Slave_station.usSRegHoldBuf[0] = 0x01;
}
uint16_t buf[2] = { 0 };

void app_task_update_poll() {
	printf("update task poll \r\n");
	//read slave
	if (Slave_station.usSRegHoldBuf[Save_Reg] == 1) {
		pid_cache.delay_ms = Slave_station.usSRegHoldBuf[PIDcal_time_ms_Reg];

		buf[0] = Slave_station.usSRegHoldBuf[Setpoint_Reg];
		buf[1] = Slave_station.usSRegHoldBuf[Setpoint_Reg + 1];
		pid_cache.sv = uint16ToFloat(buf);

		buf[0] = Slave_station.usSRegHoldBuf[Kp_Reg];
		buf[1] = Slave_station.usSRegHoldBuf[Kp_Reg + 1];
		pid_cache.kp = uint16ToFloat(buf);

		buf[0] = Slave_station.usSRegHoldBuf[Ki_Reg];
		buf[1] = Slave_station.usSRegHoldBuf[Ki_Reg + 1];
		pid_cache.ki = uint16ToFloat(buf);

		buf[0] = Slave_station.usSRegHoldBuf[Kd_Reg];
		buf[1] = Slave_station.usSRegHoldBuf[Kd_Reg + 1];
		pid_cache.kd = uint16ToFloat(buf);

		buf[0] = Slave_station.usSRegHoldBuf[Outmax_Reg];
		buf[1] = Slave_station.usSRegHoldBuf[Outmax_Reg + 1];
		pid_cache.out_max = uint16ToFloat(buf);

		buf[0] = Slave_station.usSRegHoldBuf[Outmin_Reg];
		buf[1] = Slave_station.usSRegHoldBuf[Outmin_Reg + 1];
		pid_cache.out_min = uint16ToFloat(buf);
		osDelay(100);
		// check status
		if (Slave_station.usSRegHoldBuf[Maintenance_status_Reg] == 1) {

			dlom2_set_maintenance();
			Slave_station.usSRegHoldBuf[Maintenance_status_Reg] = 0;
		}
		if (Slave_station.usSRegHoldBuf[Calib_density_Status_Reg] == 1) {
			dlom2_set_calibration_density_from_slave();
			Slave_station.usSRegHoldBuf[Calib_density_Status_Reg] = 0;
		}

		if (Slave_station.usSRegHoldBuf[Calib_viscosity_Status_Reg] == 1) {
			dlom2_set_calibration_viscosity_from_slave();
			Slave_station.usSRegHoldBuf[Calib_viscosity_Status_Reg] = 0;

		}
		osDelay(100);
	}

	//write slave
	Slave_station.usSRegHoldBuf[PWMoutput_Reg] = pwm_output;
	Slave_station.usSRegHoldBuf[PWMDir_Reg] = pwm_dir;
	osDelay(10);

	float diff = fabs(pid_cache.sv - input);
	if (diff <= 0.1) {
		Slave_station.usSRegHoldBuf[StabilizeValue_Status_Reg] = 1;
	} else

	{
		Slave_station.usSRegHoldBuf[StabilizeValue_Status_Reg] = 0;
	}
	osDelay(10);

}

